//Some math routines
//Luis Perdigao

#include <math.h>


// Geometric Algebra
// Rotors and conversion

//Multiplies geometric algebra rotors C=AB
// Assumes C values are a valid double pointer
void LPGARotorMultiplication(double A0,
							double A12,
							double A13,
							double A23,
							double B0,
							double B12,
							double B13,
							double B23,
							double *C0,
							double *C12,
							double *C13,
							double *C23)
{
	*C0=	(A0*B0)-(A12*B12)-(A13*B13)-(A23*B23);
	*C12=	(A12*B0)+(B12*A0)-(A23*B13)+(A13*B23);
	*C13=	(A13*B0)+(A23*B12)+(B13*A0)-(A12*B23);
	*C23=	(A23*B0)-(A13*B12)+(A12*B13)+(B23*A0);
	
	//Does not check if rotors are valid
}

void LPGAConvertRotationVectorToRotor(double N1,double N2,double N3,double AngleRad,
									  double *A0, double *A12,double *A13,double *A23)
{
	double temp=sin(AngleRad/2);

	*A0=	cos(AngleRad/2);
	*A12=	N3*temp;
	*A13=	-N2*temp;
	*A23=	N1*temp;

}

void LPGAConvertRotorToRotationVector(double A0, double A12, double A13, double A23,
									  double *N1, double *N2,double *N3,
									  double *AngleRad)
{
	double temp;
	
	if (A0>-1 && A0<1){
		*AngleRad=2*acos(A0);
		
		temp=sqrt(1-(A0*A0));
		if (temp!=0){		//Maybe don't need this verification due to prefious 'if'
			*N1=A23/temp;
			*N2=-A13/temp;
			*N3=A12/temp;
		}
	}else{
		*N1=1; //Default axis of rotation, zero angle
		*N2=0;
		*N3=0;
		*AngleRad=0;
	}
}